/*-----------------------------------------------------------------------*/
/* MMCv3/SDv1/SDv2 (in native mode via SDIO) control module              */
/*-----------------------------------------------------------------------*/
/* This program is opened under license policy of following trems.
/
/  Copyright (C) 2010, ChaN, all right reserved.
/  Copyright (C) 2012, Milandr.
/
/ * This program is a free software and there is NO WARRANTY.
/ * No restriction on use. You can use, modify and redistribute it for
/   personal, non-profit or commercial use UNDER YOUR RESPONSIBILITY.
/ * Redistributions of source code must retain the above copyright notice.
/
/---------------------------------------------------------------------------*/

//#include <string.h>
#include "1901BC1F.h"
#include "diskio.h"
#include "1986be9x_port.h"


/* --- SDIO configurations --- */
#define USE_4BIT  1     /* Use wide bus mode if SDC is detected */
#define PCLK    80000000UL  /* PCLK supplied to SDIO module */
#define SDIOCLK_ID    400000UL  /* SDIO for ID state (100k-400k) */
#define SDIOCLK_RW   2500000UL  /* SDIO for data transfer (PCLK divided by even number) */

/* This SDIO driver assumes that SDIO_RW is HCLK/4 or slower. */


#define INS         1 /* Socket status (true:Inserted, false:Empty) */
#define WP          0 /* Card write protection (true:yes, false:no) */


/* ----- MMC/SDC command ----- */
#define CMD0  (0)       /* GO_IDLE_STATE */
#define CMD1  (1)       /* SEND_OP_COND (MMC) */
#define CMD2  (2)       /* ALL_SEND_CID */
#define CMD3  (3)       /* SEND_RELATIVE_ADDR */
#define ACMD6 (6|0x80)    /* SET_BUS_WIDTH (SDC) */
#define CMD7  (7)       /* SELECT_CARD */
#define CMD8  (8)       /* SEND_IF_COND */
#define CMD9  (9)       /* SEND_CSD */
#define CMD10 (10)      /* SEND_CID */
#define CMD12 (12)      /* STOP_TRANSMISSION */
#define CMD13 (13)      /* SEND_STATUS */
#define ACMD13  (13|0x80)   /* SD_STATUS (SDC) */
#define CMD16 (16)      /* SET_BLOCKLEN */
#define CMD17 (17)      /* READ_SINGLE_BLOCK */
#define CMD18 (18)      /* READ_MULTIPLE_BLOCK */
#define CMD23 (23)      /* SET_BLK_COUNT (MMC) */
#define ACMD23  (23|0x80)   /* SET_WR_BLK_ERASE_COUNT (SDC) */
#define CMD24 (24)      /* WRITE_BLOCK */
#define CMD25 (25)      /* WRITE_MULTIPLE_BLOCK */
#define CMD32 (32)      /* ERASE_ER_BLK_START */
#define CMD33 (33)      /* ERASE_ER_BLK_END */
#define CMD38 (38)      /* ERASE */
#define ACMD41  (41|0x80)   /* SEND_OP_COND (SDC) */
#define CMD55 (55)      /* APP_CMD */

/* Card type flags (CardType) */
#define CT_MMC    0x01    /* MMC ver 3 */
#define CT_SD1    0x02    /* SD ver 1 */
#define CT_SD2    0x04    /* SD ver 2 */
#define CT_SDC    (CT_SD1|CT_SD2) /* SD */
#define CT_BLOCK  0x08    /* Block addressing */


/*--------------------------------------------------------------------------

   Module Private Functions

---------------------------------------------------------------------------*/
static
int wait_ready (WORD tmr);
static
int wait_ready_rcv (WORD tmr);

static volatile
DSTATUS Stat = STA_NOINIT;  /* Disk status */

static volatile
WORD Timer[2];      /* 1000Hz decrement timer for Transaction and Command */

static
WORD CardRCA;     /* Assigned RCA */
static
BYTE CardType,      /* Card type flag */
   CardInfo[16+16+4]; /* CSD(16), CID(16), OCR(4) */


static volatile
DWORD   temp_buff55[5],   /* Response return temp buffer */
        temp_buff[5];   /* Response return temp buffer */

static volatile
DWORD   tmp_DATDR,
        tmp_CRC;


/*-----------------------------------------------------------------------*/
/* Interrupt service routine for data transfer                           */
/*-----------------------------------------------------------------------*/
void Isr_SDIO (void)
{

}

/*-----------------------------------------------------------------------*/
/* Provied 8 clock cycles for the card to complete the operation         */
/*-----------------------------------------------------------------------*/
void end_8_clock (void)
{
  do {              /* Wait while CmdActive bit is set */
        SDIO->SD_CR &= 0xFFEFFFEF;  /* Reset Work2 & SBITCMD */
  } while (SDIO->SD_CR & 0x00100000);

    SDIO->SD_CR &= ~SDIO_SD_CR_CRCEN_CMD;                    /* OFF CRC_CMD */
  SDIO->SD_CR |= SDIO_SD_CR_DIRCMD;   /* Command TX */

  SDIO->SD_CMDDR  = 0x00FF;
    SDIO->SD_CMDCRC = 0x00000000;
  SDIO->SD_CMDtransfer = 8;          /* Command length */

  SDIO->SD_CR |= SDIO_SD_CR_WORK2;    /* Initiate command transaction */

  for (;;) {            /* Wait for end of the cmd transaction */
      if ((SDIO->SD_CR & SDIO_SD_CR_WORK2) == 0)
            break;  /* 8_clock End */
  }

  SDIO->SD_CR &= ~SDIO_SD_CR_DIRCMD;              /* Command RX */
    SDIO->SD_CR |=  SDIO_SD_CR_CRCEN_CMD;           /* ON CRC_CMD */
}


/*-----------------------------------------------------------------------*/
/* Receive a data packet from the MMC                                    */
/*-----------------------------------------------------------------------*/

static
int rcvr_datablock (    /* 1:OK, 0:Error */
  BYTE *buff,           /* Data buffer */
  UINT btr              /* Data block length (byte) */
)
{
    UINT i;

  SDIO->SD_CR &= ~SDIO_SD_CR_DIRDAT;              /* DATA RX */
  SDIO->SD_DATtransfer = btr*8 + (16+1);          /* Set length */
  SDIO->SD_CR |= SDIO_SD_CR_SBITDAT;
    SDIO->SD_DATA0CRC = 0x00000000;
    SDIO->SD_DATA1CRC = 0x00000000;
    SDIO->SD_DATA2CRC = 0x00000000;
    SDIO->SD_DATA3CRC = 0x00000000;
  SDIO->SD_CR |= SDIO_SD_CR_WORK;                /* Initiate data transaction */

    i=0;
  Timer[1] = 100;
  for (;;) {                /* Wait for end of the  transaction */
    if (!Timer[1]) {
            SDIO->SD_CR &= 0xFFFFFFD7;  /* Reset Work & SBITDAT */
            return 0;                   /* DATTimeOut */
        }
    if (((SDIO->SD_SR & 0x02) == 0) && (i<btr))
        {
            tmp_DATDR = SDIO->SD_DATDR;
            buff[i]   = (BYTE)(tmp_DATDR>>24 & 0x000000FF);     /* Read the response words */
            buff[i+1] = (BYTE)(tmp_DATDR>>16 & 0x000000FF);     /* Read the response words */
            buff[i+2] = (BYTE)(tmp_DATDR>>8  & 0x000000FF);     /* Read the response words */
            buff[i+3] = (BYTE)(tmp_DATDR     & 0x000000FF);     /* Read the response words */
            i=i+4;
        }
    if ((SDIO->SD_CR & SDIO_SD_CR_WORK) == 0)
            break;                      /* DATRespEnd */
  }

  while (i<btr)
    {
        tmp_DATDR = SDIO->SD_DATDR;
        buff[i]   = (BYTE)(tmp_DATDR>>24 & 0x000000FF);     /* Read the response words */
        buff[i+1] = (BYTE)(tmp_DATDR>>16 & 0x000000FF);     /* Read the response words */
        buff[i+2] = (BYTE)(tmp_DATDR>>8  & 0x000000FF);     /* Read the response words */
        buff[i+3] = (BYTE)(tmp_DATDR     & 0x000000FF);     /* Read the response words */
        i=i+4;
    }
    tmp_DATDR = SDIO->SD_DATDR;
    tmp_DATDR = SDIO->SD_DATDR;
    tmp_DATDR = SDIO->SD_DATDR;

  return 1;                             /* Function succeeded */
}


/*-----------------------------------------------------------------------*/
/* Send a data packet to the MMC                                         */
/*-----------------------------------------------------------------------*/
#if _READONLY == 0
static
int xmit_datablock (    /* 1:OK, 0:Failed */
  const BYTE *buff      /* Ponter to 512 byte data to be sent */
)
{
    UINT i;

    if (!wait_ready_rcv(500)) return 0;   /* Wait for card ready */

  SDIO->SD_CR |= SDIO_SD_CR_DIRDAT;     /* DATA TX */
    SDIO->SD_DATA0CRC = 0x00000000;
    SDIO->SD_DATA1CRC = 0x00000000;
    SDIO->SD_DATA2CRC = 0x00000000;
    SDIO->SD_DATA3CRC = 0x00000000;
    i=0;
  while (i<512)
    {
        SDIO->SD_DATDR = (DWORD)buff[i] | (DWORD)buff[i+1]<<8 | (DWORD)buff[i+2]<<16 | (DWORD)buff[i+3]<<24;
        i=i+4;
    }
  SDIO->SD_DATtransfer = 512*8  + (16+1);         /* Set length */
  SDIO->SD_CR |= SDIO_SD_CR_SBITDAT;
  SDIO->SD_CR |= SDIO_SD_CR_WORK;                 /* Initiate data transaction */

  Timer[1] = 1000;
  for (;;) {                            /* Wait for end of the resp transaction */
    if (!Timer[1]) {
            SDIO->SD_CR &= 0xFFFFFFD7;              /* Reset Work & SBITDAT */
            return 0;                               /* DATTimeOut */
        }
        if ((i==512) && !(SDIO->SD_SR & 0x08)) {    /* Add CRC & stop bit fields */
            SDIO->SD_DATDR = 0x00000000;
            i=i+2;
        }
    if ((SDIO->SD_CR & SDIO_SD_CR_WORK) == 0)
            break;                                  /* DATTransEnd */
  }
  SDIO->SD_CR &= ~SDIO_SD_CR_DIRDAT;              /* DATA RX */

    Timer[1] = 1000;
    end_8_clock();
    while ((PORTC->RXTX & 0x400) == 0) {
        end_8_clock();
    if (!Timer[1])  return 0;                   /* WriteSDTimeOut */
    }

  return 1;                                         /* Function succeeded */
}
#endif  /* _READONLY */


/*-----------------------------------------------------------------------*/
/* Stop data transfer                                                    */
/*-----------------------------------------------------------------------*/

static
void stop_transfer (void)
{
  SDIO->SD_CR &= 0xFFFE07FF;      /* Disable SDIO interrupt */
  SDIO->SD_CR &= 0xFFFFFFF7;      /* Stop SDIO data transfer */
}


/*-----------------------------------------------------------------------*/
/* Power Control (Device dependent)                                      */
/*-----------------------------------------------------------------------*/

static
int power_status (void)
{
  return (SDIO->SD_CR & 0x01) ? 1 : 0;
}


static
void power_on (void)
{
PORT_InitTypeDef PortInitStructure;

  /* Attach SDIO unit to I/O pad */
    //                             PC1 - CMD
    PortInitStructure.PORT_Pin   = PORT_Pin_1;
    PortInitStructure.PORT_MODE  = PORT_MODE_DIGITAL;
    PortInitStructure.PORT_FUNC  = PORT_FUNC_OVERRID;
    PortInitStructure.PORT_PULL_UP      = PORT_PULL_UP_ON;
    PortInitStructure.PORT_PULL_DOWN    = PORT_PULL_DOWN_OFF;
    PortInitStructure.PORT_GFEN  = PORT_GFEN_OFF;
    PortInitStructure.PORT_PD    = PORT_PD_DRIVER;
    PortInitStructure.PORT_SPEED = PORT_SPEED_MAXFAST;
    PORT_Init(PORTC, &PortInitStructure);

    //                             PC10 - D0
    PortInitStructure.PORT_Pin   = PORT_Pin_10;
    PortInitStructure.PORT_MODE  = PORT_MODE_DIGITAL;
    PortInitStructure.PORT_FUNC  = PORT_FUNC_MAIN;
    PortInitStructure.PORT_PULL_UP      = PORT_PULL_UP_ON;
    PortInitStructure.PORT_PULL_DOWN    = PORT_PULL_DOWN_OFF;
    PortInitStructure.PORT_GFEN  = PORT_GFEN_OFF;
    PortInitStructure.PORT_PD    = PORT_PD_DRIVER;
    PortInitStructure.PORT_SPEED = PORT_SPEED_MAXFAST;
    PORT_Init(PORTC, &PortInitStructure);

    //                             PC8 - CLK
    PortInitStructure.PORT_Pin   = PORT_Pin_8;
    PortInitStructure.PORT_MODE  = PORT_MODE_DIGITAL;
    PortInitStructure.PORT_FUNC  = PORT_FUNC_MAIN;
    PortInitStructure.PORT_PULL_UP      = PORT_PULL_UP_OFF;
    PortInitStructure.PORT_PULL_DOWN    = PORT_PULL_DOWN_OFF;
    PortInitStructure.PORT_GFEN  = PORT_GFEN_OFF;
    PortInitStructure.PORT_PD    = PORT_PD_DRIVER;
    PortInitStructure.PORT_SPEED = PORT_SPEED_MAXFAST;
    PORT_Init(PORTC, &PortInitStructure);

#if USE_4BIT
    //                             PC11 - D1     PC12 - D2     PC13 - D3
    PortInitStructure.PORT_Pin   = PORT_Pin_11 | PORT_Pin_12 | PORT_Pin_13;
    PortInitStructure.PORT_MODE  = PORT_MODE_DIGITAL;
    PortInitStructure.PORT_FUNC  = PORT_FUNC_MAIN;
    PortInitStructure.PORT_PULL_UP      = PORT_PULL_UP_ON;
    PortInitStructure.PORT_PULL_DOWN    = PORT_PULL_DOWN_OFF;
    PortInitStructure.PORT_GFEN  = PORT_GFEN_OFF;
    PortInitStructure.PORT_PD    = PORT_PD_DRIVER;
    PortInitStructure.PORT_SPEED = PORT_SPEED_MAXFAST;
    PORT_Init(PORTC, &PortInitStructure);
#endif

  /* Register interrupt handlers for SDIO event */
//    NVIC_SetPriority(SDIO_IRQn, 15);
//    NVIC_EnableIRQ(SDIO_IRQn);

  /* Power-on (VCC is always tied to the socket on this board) */
    SDIO->SD_CR = 0x00;
    /* Socket power on */
    SDIO->SD_CR =      SDIO_SD_CR_CLKOE     |
                       SDIO_SD_CR_CRCEN_CMD |
                       SDIO_SD_CR_CRCEN_DAT |
                       SDIO_SD_CR_SDE;
}


static
void power_off (void)
{
  SDIO->SD_CR &= 0xFFFE;      /* Power-off */

  Stat |= STA_NOINIT;
}


/*-----------------------------------------------------------------------*/
/* Send a command packet to the card and receive a response              */
/*-----------------------------------------------------------------------*/

static
int send_cmd (    /* Returns 1 when function succeeded otherwise returns 0 */
  UINT idx,   /* Command index (bit[5..0]), ACMD flag (bit7) */
  DWORD arg,    /* Command argument */
  UINT rt,    /* Expected response type. None(0), Short(1) or Long(2) */
  DWORD *buff   /* Response return buffer */
)
{

  if (idx & 0x80) {       /* Send a CMD55 prior to the specified command if it is ACMD class */
    if (!send_cmd(CMD55, (DWORD)CardRCA << 16, 1, buff) /* When CMD55 is faild, */
      || !(buff[0] & 0x00000020)) return 0;           /* exit with error */
  }
  idx &= 0x3F;          /* Mask out ACMD flag */

  do {              /* Wait while CmdActive bit is set */
        SDIO->SD_CR &= 0xFFEFFFEF;  /* Reset Work2 & SBITCMD */
  } while (SDIO->SD_CR & 0x00100000);

  SDIO->SD_CR |= SDIO_SD_CR_DIRCMD | SDIO_SD_CR_SBITCMD;   /* Command TX */

  SDIO->SD_CMDDR  = ((idx | 0x40) & 0x0000007F) |
                           (arg>>16 & 0x0000FF00) |
                           (arg     & 0x00FF0000) |
                           (arg<<16 & 0xFF000000) ;     /* Set the command + argument */
  SDIO->SD_CMDDR  = arg & 0x000000FF;                 /* Set the argument */
    SDIO->SD_CMDCRC = 0x00000000;
  SDIO->SD_CMDtransfer = 48;          /* Command length */

  SDIO->SD_CR |= SDIO_SD_CR_WORK2;    /* Initiate command transaction */

  Timer[1] = 100;
  for (;;) {                /* Wait for end of the cmd transaction */
    if (!Timer[1]) return 0;        /* CmdTimeOut */
    if (rt == 0) {
        if ((SDIO->SD_CR & SDIO_SD_CR_WORK2) == 0) {
                return 1;             /* CmdSent */
            }
    } else {
        if ((SDIO->SD_CR & SDIO_SD_CR_WORK2) == 0)
                break;                  /* CmdRespEnd */
    }
  }

  for (Timer[1] = 2; Timer[1]; );

  SDIO->SD_CR &= ~SDIO_SD_CR_DIRCMD;              /* Command RX */
  if (rt == 1)    SDIO->SD_CMDtransfer = 48;    /* Set Response bit to reveice short resp */
  if (rt > 1)     SDIO->SD_CMDtransfer = 136;   /* Set Response and LongResp bit to receive long resp */
  SDIO->SD_CR |= SDIO_SD_CR_SBITCMD;
    SDIO->SD_CMDCRC = 0x00000000;
  SDIO->SD_CR |= SDIO_SD_CR_WORK2 ;               /* Initiate command transaction resp */

  Timer[1] = 100;
  for (;;) {                /* Wait for end of the resp transaction */
    if (!Timer[1]) {
            SDIO->SD_CR &= 0xFFEFFFEF;  /* Reset Work2 & SBITCMD */
            return 0;                   /* CmdTimeOut */
        }
      if ((SDIO->SD_CR & SDIO_SD_CR_WORK2) == 0) {
            break;                      /* CmdRespEnd */
        }
  }

    temp_buff[0] = SDIO->SD_CMDDR;          /* Read the response words 0 */
    temp_buff[1] = SDIO->SD_CMDDR;          /* Read the response words 1 */

  buff[0] = ((temp_buff[0]<<8) & 0xFFFFFF00) | ((temp_buff[1]>>8) & 0x000000FF);
  if (rt == 2) {
        temp_buff[2] = SDIO->SD_CMDDR;      /* Read the response words 2 */
        temp_buff[3] = SDIO->SD_CMDDR;      /* Read the response words 3 */
        temp_buff[4] = SDIO->SD_CMDDR;      /* Read the response words 4 */
      buff[1] = ((temp_buff[1]<<8) & 0xFFFFFF00) | ((temp_buff[2]>>8) & 0x000000FF);
      buff[2] = ((temp_buff[2]<<8) & 0xFFFFFF00) | ((temp_buff[3]>>8) & 0x000000FF);
      buff[3] = ((temp_buff[3]<<8) & 0xFFFFFF00) | ((temp_buff[4]>>8) & 0x000000FF);
  }

  return 1;                           /* Return with success */
}


/*-----------------------------------------------------------------------*/
/* Wait card ready                                                       */
/*-----------------------------------------------------------------------*/

static
int wait_ready (  /* Returns 1 when card is tran state, otherwise returns 0 */
  WORD tmr    /* Timeout in unit of 1ms */
)
{
  DWORD rc;


  Timer[0] = tmr;
  while (Timer[0]) {
    if (send_cmd(CMD13, (DWORD)CardRCA << 16, 1, &rc)
      && ((rc & 0x01E00) == 0x00800)) break;
    /* This loop will take a time. Insert rot_rdq() here for multitask envilonment. */
  }
  return Timer[0] ? 1 : 0;
}

/*-----------------------------------------------------------------------*/
/* Wait card ready for rcv                                               */
/*-----------------------------------------------------------------------*/

static
int wait_ready_rcv (  /* Returns 1 when card is rcv state, otherwise returns 0 */
  WORD tmr        /* Timeout in unit of 1ms */
)
{
  DWORD rc;


  Timer[0] = tmr;
  while (Timer[0]) {
    if (send_cmd(CMD13, (DWORD)CardRCA << 16, 1, &rc)
      && ((rc & 0x01F00) == 0x00D00)) break;
    /* This loop will take a time. Insert rot_rdq() here for multitask envilonment. */
  }
  return Timer[0] ? 1 : 0;
}


/*-----------------------------------------------------------------------*/
/* Swap byte order                                                       */
/*-----------------------------------------------------------------------*/

static
void bswap_cp (BYTE *dst, const DWORD *src)
{
  DWORD d;


  d = *src;
  *dst++ = (BYTE)(d >> 24);
  *dst++ = (BYTE)(d >> 16);
  *dst++ = (BYTE)(d >> 8);
  *dst++ = (BYTE)(d >> 0);
}


/*--------------------------------------------------------------------------

   Public Functions

---------------------------------------------------------------------------*/


/*-----------------------------------------------------------------------*/
/* Initialize Disk Drive                                                 */
/*-----------------------------------------------------------------------*/

DSTATUS disk_initialize (BYTE drv)
{
  UINT cmd, n;
  DWORD resp[4];
  BYTE ty;


  if (Stat & STA_NODISK) return Stat;   /* No card in the socket */

  power_on();               /* Force socket power on */

    SDIO->SD_CR |= SDIO_SD_CR_CLKOE | 7<<SDIO_SD_CR_BR0_Pos;    /* Set SDIOCLK = SDIOCLK_ID */

  for (Timer[0] = 2; Timer[0]; );
  
#define NULL 0
  send_cmd(CMD0, 0, 0, NULL);       /* Enter idle state */
  CardRCA = 0;

  /*---- Card is 'idle' state ----*/

  Timer[0] = 1000;            /* Initialization timeout of 1000 msec */
  if (send_cmd(CMD8, 0x1AA, 1, resp)    /* SDC Ver2 */
    && (resp[0] & 0xFFF) == 0x1AA) {    /* The card can work at vdd range of 2.7-3.6V */
    do {                  /* Wait while card is busy state (use ACMD41 with HCS bit) */
      /* This loop will take a time. Insert wai_tsk(1) here for multitask envilonment. */
      if (!Timer[0]) goto di_fail;
    } while (!send_cmd(ACMD41, 0x40FF8000, 1, resp) || !(resp[0] & 0x80000000));
    ty = (resp[0] & 0x40000000) ? CT_SD2|CT_BLOCK : CT_SD2; /* Check CCS bit in the OCR */
  }
  else {                  /* SDC Ver1 or MMC */
    if (send_cmd(ACMD41, 0x00FF8000, 1, resp)) {
      ty = CT_SD1; cmd = ACMD41;      /* ACMD41 is accepted -> SDC Ver1 */
    } else {
      ty = CT_MMC; cmd = CMD1;      /* ACMD41 is rejected -> MMC */
    }
    do {                  /* Wait while card is busy state (use ACMD41 or CMD1) */
      /* This loop will take a time. Insert wai_tsk(1) here for multitask envilonment. */
      if (!Timer[0]) goto di_fail;
    } while (!send_cmd(cmd, 0x00FF8000, 1, resp) || !(resp[0] & 0x80000000));
  }

  CardType = ty;              /* Save card type */
  bswap_cp(&CardInfo[32], resp);      /* Save OCR */

  /*---- Card is 'ready' state ----*/

  if (!send_cmd(CMD2, 0, 2, resp)) goto di_fail;  /* Enter ident state */
  for (n = 0; n < 4; n++) bswap_cp(&CardInfo[n * 4 + 16], &resp[n]);  /* Save CID */

  /*---- Card is 'ident' state ----*/

  if (ty & CT_SDC) {            /* SDC: Get generated RCA and save it */
    if (!send_cmd(CMD3, 0, 1, resp)) goto di_fail;
    CardRCA = (WORD)(resp[0] >> 16);
  } else {                /* MMC: Assign RCA to the card */
    if (!send_cmd(CMD3, 1 << 16, 1, resp)) goto di_fail;
    CardRCA = 1;
  }

  /*---- Card is 'stby' state ----*/

  if (!send_cmd(CMD9, (DWORD)CardRCA << 16, 2, resp))   /* Get CSD and save it */
    goto di_fail;
  for (n = 0; n < 4; n++) bswap_cp(&CardInfo[n * 4], &resp[n]);

  if (!send_cmd(CMD7, (DWORD)CardRCA << 16, 1, resp))   /* Select card */
    goto di_fail;

  /*---- Card is 'tran' state ----*/

  if (!(ty & CT_BLOCK)) {   /* Set data block length to 512 (for byte addressing cards) */
    if (!send_cmd(CMD16, 512, 1, resp) || (resp[0] & 0xFDF90000))
      goto di_fail;
  }

#if USE_4BIT
  if (ty & CT_SDC) {    /* Set wide bus mode (for SDCs) */
    if (!send_cmd(ACMD6, 2, 1, resp)  /* Set bus mode of SDC */
      || (resp[0] & 0xFDF90000))
      goto di_fail;
//        SDIO->SD_CR &= ~SDIO_SD_CR_WIDTHDAT; /* Set bus mode */
  }
#else
        SDIO->SD_CR |= SDIO_SD_CR_WIDTHDAT; /* Set bus mode */
#endif

    /* Set SDIOCLK = SDIOCLK_RW */
    SDIO->SD_CR &= ~(5<<SDIO_SD_CR_BR0_Pos);   /* Set bus speed HCLK/8 */

  Stat &= ~STA_NOINIT;  /* Clear STA_NOINIT */
  return Stat;

di_fail:
  power_off();
  Stat |= STA_NOINIT;   /* Set STA_NOINIT */
  return Stat;
}


/*-----------------------------------------------------------------------*/
/* Get Disk Status                                                       */
/*-----------------------------------------------------------------------*/

DSTATUS disk_status (BYTE drv)
{
  return Stat;
}


/*-----------------------------------------------------------------------*/
/* Read Sector(s)                                                        */
/*-----------------------------------------------------------------------*/

DRESULT disk_read (
  BYTE drv,
  BYTE *buff,     /* Pointer to the data buffer to store read data */
  DWORD sector,   /* Start sector number (LBA) */
  BYTE count      /* Sector count (1..127) */
)
{
  DWORD resp;
  UINT cmd;


  if (count < 1 || count > 127) return RES_PARERR;  /* Check parameter */
  if (Stat & STA_NOINIT) return RES_NOTRDY; /* Check drive status */

  if (!(CardType & CT_BLOCK)) sector *= 512;  /* Convert LBA to byte address if needed */
  if (!wait_ready(500)) return RES_ERROR;   /* Make sure that card is tran state */

  cmd = (count > 1) ? CMD18 : CMD17;    /* Transfer type: Single block or Multiple block */
  if (send_cmd(cmd, sector, 1, &resp)   /* Start to read */
    && !(resp & 0xC0580000)) {
    do {
            if (!rcvr_datablock(buff, 512)) break;
      buff += 512;            /* Next user buffer address */
    } while (--count);
        end_8_clock();
    if (cmd == CMD18)           /* Terminate to read (MB) */
      send_cmd(CMD12, 0, 1, &resp);
  }

  stop_transfer();            /* Close data path */

  return count ? RES_ERROR : RES_OK;
}


/*-----------------------------------------------------------------------*/
/* Write Sector(s)                                                       */
/*-----------------------------------------------------------------------*/

#if _READONLY == 0
DRESULT disk_write (
  BYTE drv,
  const BYTE *buff, /* Pointer to the data to be written */
  DWORD sector,   /* Start sector number (LBA) */
  BYTE count      /* Sector count (1..127) */
)
{
  DWORD rc;
  UINT cmd;

  if (count < 1 || count > 127) return RES_PARERR;  /* Check parameter */
  if (Stat & STA_NOINIT) return RES_NOTRDY; /* Check drive status */
  if (Stat & STA_PROTECT) return RES_WRPRT; /* Check write protection */

  if (!(CardType & CT_BLOCK)) sector *= 512;  /* Convert LBA to byte address if needed */
  if (!wait_ready(500)) return RES_ERROR;   /* Make sure that card is tran state */

  if (count == 1) {                           /* Single block write */
    cmd = CMD24;
  } else {                                    /* Multiple block write */
    cmd = (CardType & CT_SDC) ? ACMD23 : CMD23;
    if (!send_cmd(cmd, count, 1, &rc)   /* Preset number of blocks to write */
      || (rc & 0xC0580000)) {
      return RES_ERROR;
    }
    cmd = CMD25;
  }

  if (!send_cmd(cmd, sector, 1, &rc)      /* Send a write command */
    || (rc & 0xC0580000)) {
    return RES_ERROR;
  }

    while (count) {
        if (!xmit_datablock(buff)) break;
    count--; buff += 512;           /* Next user buffer address */
    }

  stop_transfer();              /* Close data path */
  if (cmd == CMD25 && (CardType & CT_SDC))  /* Terminate to write (SDC w/MB) */
    send_cmd(CMD12, 0, 1, &rc);

  return count ? RES_ERROR : RES_OK;
}
#endif /* _READONLY */


/*-----------------------------------------------------------------------*/
/* Miscellaneous Functions                                               */
/*-----------------------------------------------------------------------*/

DRESULT disk_ioctl (
  BYTE drv,
  BYTE ctrl,    /* Control code */
  void *buff    /* Buffer to send/receive data block */
)
{
  DRESULT res;
  BYTE b, *ptr = buff;
  DWORD resp[4], d, *dp, st, ed;


  if (Stat & STA_NOINIT) return RES_NOTRDY;

  res = RES_ERROR;

  switch (ctrl) {
    case CTRL_SYNC :  /* Make sure that all data has been written on the media */
      if (wait_ready(500))  /* Wait for card enters tarn state */
        res = RES_OK;
      break;

    case GET_SECTOR_COUNT : /* Get number of sectors on the disk (DWORD) */
      if ((CardInfo[0] >> 6) == 1) {  /* SDC CSD v2.0 */
        d = ((WORD)CardInfo[8] << 8) + CardInfo[9] + 1;
        *(DWORD*)buff = d << 10;
      } else {            /* MMC or SDC CSD v1.0 */
        b = (CardInfo[5] & 15) + ((CardInfo[10] & 128) >> 7) + ((CardInfo[9] & 3) << 1) + 2;
        d = (CardInfo[8] >> 6) + ((WORD)CardInfo[7] << 2) + ((WORD)(CardInfo[6] & 3) << 10) + 1;
        *(DWORD*)buff = d << (b - 9);
      }
      res = RES_OK;
      break;

    case GET_BLOCK_SIZE : /* Get erase block size in unit of sectors (DWORD) */
      if (CardType & CT_SD2) {  /* SDC ver 2.00 */
        *(DWORD*)buff = 16UL << (CardInfo[10] >> 4);
      } else {          /* SDC ver 1.XX or MMC */
        if (CardType & CT_SD1)  /* SDC v1 */
          *(DWORD*)buff = (((CardInfo[10] & 63) << 1) + ((WORD)(CardInfo[11] & 128) >> 7) + 1) << ((CardInfo[13] >> 6) - 1);
        else          /* MMC */
          *(DWORD*)buff = ((WORD)((CardInfo[10] & 124) >> 2) + 1) * (((CardInfo[11] & 3) << 3) + ((CardInfo[11] & 224) >> 5) + 1);
      }
      res = RES_OK;
      break;

    case CTRL_ERASE_SECTOR :  /* Erase a block of sectors */
      if (!(CardType & CT_SDC) || (!(CardInfo[0] >> 6) && !(CardInfo[10] & 0x40))) break; /* Check if sector erase can be applied to the card */
      dp = buff; st = dp[0]; ed = dp[1];
      if (!(CardType & CT_BLOCK)) {
        st *= 512; ed *= 512;
      }
      if (send_cmd(CMD32, st, 1, resp) && send_cmd(CMD33, ed, 1, resp) && send_cmd(CMD38, 0, 1, resp) && wait_ready(30000))
        res = RES_OK;
      break;

    case CTRL_POWER :
      switch (ptr[0]) {
      case 0:   /* Sub control code == 0 (POWER_OFF) */
        power_off();    /* Power off */
        res = RES_OK;
        break;
      case 1:   /* Sub control code == 1 (POWER_GET) */
        ptr[1] = (BYTE)power_status();
        res = RES_OK;
        break;
      default :
        res = RES_PARERR;
      }
      break;

    case MMC_GET_TYPE :   /* Get card type flags (1 byte) */
      *ptr = CardType;
      res = RES_OK;
      break;

    case MMC_GET_CSD :    /* Get CSD (16 bytes) */
      memcpy(buff, &CardInfo[0], 16);
      res = RES_OK;
      break;

    case MMC_GET_CID :    /* Get CID (16 bytes) */
      memcpy(buff, &CardInfo[16], 16);
      res = RES_OK;
      break;

    case MMC_GET_OCR :    /* Get OCR (4 bytes) */
      memcpy(buff, &CardInfo[32], 4);
      res = RES_OK;
      break;

    case MMC_GET_SDSTAT : /* Receive SD status as a data block (64 bytes) */
      if (CardType & CT_SDC) {  /* SDC */
        if (wait_ready(500)) {
          if (send_cmd(ACMD13, 0, 1, resp)  /* Start to read */
              && !(resp[0] & 0xC0580000)) {
                            if (rcvr_datablock(buff, 512)) res = RES_OK;
          }
        }
        stop_transfer();          /* Close data path */
      }
      break;

    default:
      res = RES_PARERR;
  }

  return res;
}


/*-----------------------------------------------------------------------*/
/* Device timer function  (Platform dependent)                           */
/*-----------------------------------------------------------------------*/
/* This function must be called from timer interrupt routine in period
/  of 1 ms (0.1 ms) to generate card control timing.
*/

void disk_timerproc (void)
{
    WORD n;
    BYTE s;


    n = Timer[0];           /* 1kHz (10kHz) decrement timer stopped at 0 */
    if (n) Timer[0] = --n;
    n = Timer[1];
    if (n) Timer[1] = --n;

    s = Stat;
    if (WP)   /* Write protected */
        s |= STA_PROTECT;
    else    /* Write enabled */
        s &= ~STA_PROTECT;
    if (INS)  /* Card is in socket */
        s &= ~STA_NODISK;
    else    /* Socket empty */
        s |= (STA_NODISK | STA_NOINIT);
    Stat = s;
}

